
#include <io.h>
#include <direct.h>
#include <iostream>

#include "fmiAdapter.h"
#include "tinyxml2.h"
#include "stringSplit.h"
#include "spckAdapter.h"


#define BUFFER 1000
#define RETURN_FAIL 1
#define RETURN_SUCCESS 0

using namespace std;

C3x3Matrix ort2matrix(C3dVector ort)
{
	double al = ort.x();
	double be = ort.y();
	double ga = ort.z();

	C3x3Matrix rot_al(1, 0, 0,
		0, cos(al), -sin(al),
		0, sin(al), cos(al));
	C3x3Matrix rot_be(cos(be), 0, sin(be),
		0, 1, 0,
		-sin(be), 0, cos(be));
	C3x3Matrix rot_ga(cos(ga), -sin(ga), 0,
		sin(ga), cos(ga), 0,
		0, 0, 1);

	C3x3Matrix m_rot = rot_ga * rot_be * rot_al;

	return m_rot;

}
struct ModelVariable
{
	string name;
	int valueReference;
	string causality;
};

void importlogger(jm_callbacks* c, jm_string module, jm_log_level_enu_t log_level, jm_string message)
{
	printf("%s: %s\n", jm_log_level_to_string(log_level), message);
}

FMIAdapter::FMIAdapter(string fmuFile, string spckFile)
{
	FMUFile = fmuFile;
	SpckFile = spckFile;
	vector<string> pathStrings = StringSplit::SplitWithStl(fmuFile, "\\");
	string fmuTempPath;
	for (int i = 0; i < pathStrings.size() - 1; i++)
	{
		fmuTempPath = fmuTempPath+pathStrings[i]+ "\\";
	}
	fmuTempPath += "temp";
	
	FMUTempPath = fmuTempPath;
	if (0 != _access(FMUTempPath.c_str(), 0))
	{
		// if this folder not exist, create a new one.
		_mkdir(FMUTempPath.c_str()); 
	}
	
}


vector<CosimBody> FMIAdapter::getCosimBody()
{
	vector<CosimBody> cosimBodyGroup;
	string xmlDocPath = FMUTempPath + "\\modelDescription.xml";
	tinyxml2::XMLDocument doc;
	doc.LoadFile(xmlDocPath.c_str());
	if (doc.Error())
	{
		printf("can not load fmu temp file!");
	}

	tinyxml2::XMLElement* root = doc.RootElement();
	tinyxml2::XMLElement* variables = root->FirstChildElement("ModelVariables");
	tinyxml2::XMLElement* variable = variables->FirstChildElement();

	//Get the fmi ModelVariables 
	vector<FmiVariable> fmiVariables;
	while (variable)
	{
		FmiVariable fmiVariable = {"",0,""};
		fmiVariable.name = variable->Attribute("name");
		fmiVariable.valueReference = atoi(variable->Attribute("valueReference"));
		fmiVariable.causality = variable->Attribute("causality");
		fmiVariables.push_back(fmiVariable);
		variable = variable->NextSiblingElement();		
	}

	//Get cosim bodies of simpack intput file.
	SpckAdapter spckAdapter(SpckFile);
	vector<SpckCosimBody> CosimBodies =  spckAdapter.GetSpckCosimBody();

	for (int i = 0; i < CosimBodies.size(); i++)
	{
		CosimBody cosimBody = { 0,0,"",{},0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 };
		SpckCosimBody spckCosimBody = CosimBodies[i];
		cosimBody.name = StringSplit::DeleteSub(spckCosimBody.name, "$B_");
		memcpy(cosimBody.momentOfInertia, spckCosimBody.momentOfInertia, sizeof(spckCosimBody.momentOfInertia));
		cosimBody.mass = spckCosimBody.mass;
		cosimBody.id = i + 1;
		for (int j = 0; j < fmiVariables.size(); j++)
		{
			if (fmiVariables[j].name == spckCosimBody.pos_x_name)
			{
				cosimBody.pos_x = fmiVariables[j].valueReference;
			}
			else if (fmiVariables[j].name == spckCosimBody.pos_y_name)
			{
				cosimBody.pos_y = fmiVariables[j].valueReference;
			}
			else if (fmiVariables[j].name == spckCosimBody.pos_z_name)
			{
				cosimBody.pos_z = fmiVariables[j].valueReference;
			}
			else if (fmiVariables[j].name == spckCosimBody.ort_x_name)
			{
				cosimBody.ort_x = fmiVariables[j].valueReference;
			}
			else if (fmiVariables[j].name == spckCosimBody.ort_y_name)
			{
				cosimBody.ort_y = fmiVariables[j].valueReference;
			}
			else if (fmiVariables[j].name == spckCosimBody.ort_z_name)
			{
				cosimBody.ort_z = fmiVariables[j].valueReference;
			}
			else if (fmiVariables[j].name == spckCosimBody.vel_x_name)
			{
				cosimBody.vel_x = fmiVariables[j].valueReference;
			}
			else if (fmiVariables[j].name == spckCosimBody.vel_y_name)
			{
				cosimBody.vel_y = fmiVariables[j].valueReference;
			}
			else if (fmiVariables[j].name == spckCosimBody.vel_z_name)
			{
				cosimBody.vel_z = fmiVariables[j].valueReference;
			}
			else if (fmiVariables[j].name == spckCosimBody.ang_vel_x_name)
			{
				cosimBody.ang_vel_x = fmiVariables[j].valueReference;
			}
			else if (fmiVariables[j].name == spckCosimBody.ang_vel_y_name)
			{
				cosimBody.ang_vel_y = fmiVariables[j].valueReference;
			}
			else if (fmiVariables[j].name == spckCosimBody.ang_vel_z_name)
			{
				cosimBody.ang_vel_z = fmiVariables[j].valueReference;
			}
			else if (fmiVariables[j].name == spckCosimBody.fx_name)
			{
				cosimBody.fx = fmiVariables[j].valueReference;
			}
			else if (fmiVariables[j].name == spckCosimBody.fy_name)
			{
				cosimBody.fy = fmiVariables[j].valueReference;
			}
			else if (fmiVariables[j].name == spckCosimBody.fz_name)
			{
				cosimBody.fz = fmiVariables[j].valueReference;
			}
			else if (fmiVariables[j].name == spckCosimBody.tx_name)
			{
				cosimBody.tx = fmiVariables[j].valueReference;
			}
			else if (fmiVariables[j].name == spckCosimBody.ty_name)
			{
				cosimBody.ty = fmiVariables[j].valueReference;
			}
			else if (fmiVariables[j].name == spckCosimBody.tz_name)
			{
				cosimBody.tz = fmiVariables[j].valueReference;
			}
		}
		cosimBodyGroup.push_back(cosimBody);

	}

	return cosimBodyGroup;
	
	
}


bool FMIAdapter::RunSimulation()
{
	

	// Initialise the EDEM coupling
	bool init_edem_status = false;
	// Set the box properties

	//**********************************************************************************
	/////////////////////////////EDEM Coupling Initialisation //////////////////////////
	//**********************************************************************************

	IEDEMCoupling coupling;

	if (!coupling.initialiseCoupling())
	{
		cout << "Can't initialise the EDEM Coupling Client" << endl;
		init_edem_status = false;
		return false;
	}
	cout << "EDEM Coupling Client initialised" << endl << "Connecting to EDEM..." << endl;

	// Connect to EDEM
	if (!coupling.connectCoupling())
	{
		cout << "Could not connect to EDEM" << endl;
		init_edem_status = false;
		return false;
	}
	cout << "Connection to EDEM successful" << endl;

	////////////////////////// Obtain Simulation Variables /////////////////////////////

	const double curtime = 0;
	coupling.setEDEMTime(curtime);

	double dt;
	coupling.getEDEMTimeStep(dt);

	double totaltime;
	coupling.getTotalSimulationTime(totaltime);

	//dt *= DATA_EXCHANGE_RATIO;

	//**********************************************************************************
	/////////////////////////////FMI Coupling Initialisation ///////////////////////////
	//**********************************************************************************

	bool init_fmi_status = false;

	fmi2_callback_functions_t callBackFunctions;
	jm_callbacks callbacks;
	fmi2_import_t* fmu;
	fmi_import_context_t* context;
	fmi_version_enu_t version;
	jm_status_enu_t status;
	fmi2_status_t fmistatus;


	callbacks.malloc = malloc;
	callbacks.calloc = calloc;
	callbacks.realloc = realloc;
	callbacks.free = free;
	callbacks.logger = importlogger;
	callbacks.log_level = jm_log_level_debug;
	callbacks.context = 0;

	context = fmi_import_allocate_context(&callbacks);

	version = fmi_import_get_fmi_version(context, FMUFile.c_str(), FMUTempPath.c_str());

	if (version != fmi_version_2_0_enu) {
		printf("The code only supports version 2.0\n");
		init_fmi_status = false;
		return false;
	}

	fmu = fmi2_import_parse_xml(context, FMUTempPath.c_str(), 0);

	if (!fmu) {
		printf("Error parsing XML, exiting\n");
		init_fmi_status = false;
		return false;
	}

	if (fmi2_import_get_fmu_kind(fmu) == fmi2_fmu_kind_me) {
		printf("Only CS 2.0 is supported by this code\n");
		init_fmi_status = false;
		return false;
	}

	callBackFunctions.logger = fmi2_log_forwarding;
	callBackFunctions.allocateMemory = calloc;
	callBackFunctions.freeMemory = free;
	callBackFunctions.componentEnvironment = fmu;

	status = fmi2_import_create_dllfmu(fmu, fmi2_fmu_kind_cs, &callBackFunctions);
	if (status == jm_status_error) {
		printf("Could not create the DLL loading mechanism(C-API) (error: %s).\n", fmi2_import_get_last_error(fmu));
		init_fmi_status = false;
		return false;
	}

	fmi2_string_t instanceName = "Test CS model instance";
	fmi2_string_t fmuLocation = "";
	fmi2_boolean_t visible = fmi2_false;
	fmi2_real_t relativeTol = 1e-4;

	fmi2_real_t tstart = 0.0;
	fmi2_real_t tcur = tstart;
	fmi2_real_t hstep = 0.1;
	fmi2_real_t tend = totaltime;
	fmi2_boolean_t StopTimeDefined = fmi2_false;

	printf("Version returned from FMU:   %s\n", fmi2_import_get_version(fmu));
	printf("Platform type returned:      %s\n", fmi2_import_get_types_platform(fmu));

	////fmuGUID = fmi2_import_get_GUID(fmu);
	////printf("GUID:      %s\n", fmuGUID);

	status = fmi2_import_instantiate(fmu, instanceName, fmi2_cosimulation, fmuLocation, visible);
	if (status == jm_status_error) {
		printf("fmi2_import_instantiate failed\n");
		init_fmi_status = false;
		return false;
	}

	fmistatus = fmi2_import_setup_experiment(fmu, fmi2_true,
		relativeTol, tstart, StopTimeDefined, tend);
	if (fmistatus != fmi2_status_ok) {
		printf("fmi2_import_setup_experiment failed\n");
		init_fmi_status = false;
		return false;
	}

	fmistatus = fmi2_import_enter_initialization_mode(fmu);
	if (fmistatus != fmi2_status_ok) {
		printf("fmi2_import_enter_initialization_mode failed\n");
		init_fmi_status = false;
		return false;
	}

	fmistatus = fmi2_import_exit_initialization_mode(fmu);
	if (fmistatus != fmi2_status_ok) {
		printf("fmi2_import_exit_initialization_mode failed\n");
		init_fmi_status = false;
		return false;
	}

	init_fmi_status = true;

	
	vector<CosimBody> CosimBodyGroup = getCosimBody();//Get Coupling Variables 

	//**********************************************************************************
	///////////////////////////// Coupling  simultion //////////////////////////////////
	//**********************************************************************************

	CGeometry box;

	for (int i = 0; i < CosimBodyGroup.size(); i++)
	{
		box.mass = CosimBodyGroup[i].mass; // kg    
		box.momentOfInertia = C3x3Matrix(CosimBodyGroup[i].momentOfInertia[0][0], CosimBodyGroup[i].momentOfInertia[0][1], CosimBodyGroup[i].momentOfInertia[0][2], 
			CosimBodyGroup[i].momentOfInertia[1][0], CosimBodyGroup[i].momentOfInertia[1][1], CosimBodyGroup[i].momentOfInertia[1][2], 
			CosimBodyGroup[i].momentOfInertia[2][0], CosimBodyGroup[i].momentOfInertia[2][1], CosimBodyGroup[i].momentOfInertia[2][2] );

		if (coupling.getGeometryId(CosimBodyGroup[i].name.c_str(), CosimBodyGroup[i].id))
		{
			cout << "Found the geometry " << CosimBodyGroup[i].name << "." << endl;
		}

		// Get the initial coupling position and orientation from EDEM
		//coupling.getGeometryPosition(box.id, box.centerOfMass, box.orientation);
		
	}

	cout << "Run the coupling simulation." << endl;
	double simTime = 0;
	while (simTime <= tend)
	{
		fmi2_boolean_t newStep = fmi2_true;

		fmi2_real_t rv_pos_x;
		fmi2_real_t rv_pos_y;
		fmi2_real_t rv_pos_z;

		fmi2_real_t rv_ort_x;
		fmi2_real_t rv_ort_y;
		fmi2_real_t rv_ort_z;

		fmi2_real_t rv_vel_x;
		fmi2_real_t rv_vel_y;
		fmi2_real_t rv_vel_z;

		fmi2_real_t rv_ang_vel_x;
		fmi2_real_t rv_ang_vel_y;
		fmi2_real_t rv_ang_vel_z;

		double rv_fx;
		double rv_fy;
		double rv_fz;

		double rv_tx;
		double rv_ty;
		double rv_tz;

		fmi2_value_reference_t fx;
		fmi2_value_reference_t fy;
		fmi2_value_reference_t fz;

		fmi2_value_reference_t tx;
		fmi2_value_reference_t ty;
		fmi2_value_reference_t tz;

		fmi2_value_reference_t pos_x;
		fmi2_value_reference_t pos_y;
		fmi2_value_reference_t pos_z;

		fmi2_value_reference_t ort_x;
		fmi2_value_reference_t ort_y;
		fmi2_value_reference_t ort_z;

		fmi2_value_reference_t vel_x;
		fmi2_value_reference_t vel_y;
		fmi2_value_reference_t vel_z;

		fmi2_value_reference_t ang_vel_x;
		fmi2_value_reference_t ang_vel_y;
		fmi2_value_reference_t ang_vel_z;

		for (int i = 0; i < CosimBodyGroup.size(); i++)
		{
			pos_x = CosimBodyGroup[i].pos_x;
			pos_y = CosimBodyGroup[i].pos_y;
			pos_z = CosimBodyGroup[i].pos_z;

			ort_x = CosimBodyGroup[i].ort_x;
			ort_y = CosimBodyGroup[i].ort_y;
			ort_z = CosimBodyGroup[i].ort_z;

			vel_x = CosimBodyGroup[i].vel_x;
			vel_y = CosimBodyGroup[i].vel_y;
			vel_z = CosimBodyGroup[i].vel_z;

			ang_vel_x = CosimBodyGroup[i].ang_vel_x;
			ang_vel_y = CosimBodyGroup[i].ang_vel_y;
			ang_vel_z = CosimBodyGroup[i].ang_vel_z;

			fmistatus = fmi2_import_get_real(fmu, &pos_x, 1, &rv_pos_x);
			fmistatus = fmi2_import_get_real(fmu, &pos_y, 1, &rv_pos_y);
			fmistatus = fmi2_import_get_real(fmu, &pos_z, 1, &rv_pos_z);

			fmistatus = fmi2_import_get_real(fmu, &ort_x, 1, &rv_ort_x);
			fmistatus = fmi2_import_get_real(fmu, &ort_y, 1, &rv_ort_y);
			fmistatus = fmi2_import_get_real(fmu, &ort_z, 1, &rv_ort_z);

			fmistatus = fmi2_import_get_real(fmu, &vel_x, 1, &rv_vel_x);
			fmistatus = fmi2_import_get_real(fmu, &vel_y, 1, &rv_vel_y);
			fmistatus = fmi2_import_get_real(fmu, &vel_z, 1, &rv_vel_z);

			fmistatus = fmi2_import_get_real(fmu, &ang_vel_x, 1, &rv_ang_vel_x);
			fmistatus = fmi2_import_get_real(fmu, &ang_vel_y, 1, &rv_ang_vel_y);
			fmistatus = fmi2_import_get_real(fmu, &ang_vel_z, 1, &rv_ang_vel_z);			

			box.id = CosimBodyGroup[i].id;			
			
			box.totalTranslation = C3dVector(rv_pos_x, rv_pos_y, rv_pos_z);
			box.orientation = ort2matrix(C3dVector(rv_ort_x, rv_ort_y, rv_ort_z));
			box.velocity = C3dVector(rv_vel_x, rv_vel_y, rv_vel_z);
			box.angularVelocity = C3dVector(rv_ang_vel_x, rv_ang_vel_y, rv_ang_vel_z);

			// Send motion data to EDEM before simulating the time-step
			coupling.setGeometryMotion(box.id,
				box.totalTranslation,
				box.orientation,
				box.velocity,
				box.angularVelocity,
				dt,
				true);

			C3dVector m_force; C3dVector m_torque;
			coupling.getGeometryForces(box.id, m_force, m_torque);
			if (CosimBodyGroup[i].fx > 0)
			{
				rv_fx = m_force.x();
				fx = CosimBodyGroup[i].fx;
				fmistatus = fmi2_import_set_real(fmu, &fx, 1, &rv_fx);
			}
			if (CosimBodyGroup[i].fy > 0)
			{
				rv_fy = m_force.y();
				fy = CosimBodyGroup[i].fy;
				fmistatus = fmi2_import_set_real(fmu, &fy, 1, &rv_fy);
			}
			if (CosimBodyGroup[i].fz > 0)
			{
				rv_fz = m_force.z();
				fz = CosimBodyGroup[i].fz;
				fmistatus = fmi2_import_set_real(fmu, &fz, 1, &rv_fz);
			}
			if (CosimBodyGroup[i].tx > 0)
			{
				rv_tx = m_torque.x();
				tx = CosimBodyGroup[i].tx;
				fmistatus = fmi2_import_set_real(fmu, &tx, 1, &rv_tx);
			}
			if (CosimBodyGroup[i].ty > 0)
			{
				rv_ty = m_torque.y();
				ty = CosimBodyGroup[i].ty;
				fmistatus = fmi2_import_set_real(fmu, &ty, 1, &rv_ty);
			}
			if (CosimBodyGroup[i].tz > 0)
			{
				rv_tz = m_torque.z();
				tz = CosimBodyGroup[i].tz;
				fmistatus = fmi2_import_set_real(fmu, &tz, 1, &rv_tz);
			}
		}
		
		coupling.simulate(dt, tend);
		fmistatus = fmi2_import_do_step(fmu, simTime, dt, newStep);

		simTime += dt;

	}

	//**********************************************************************************
	/////////////////////////////close  Coupling  simultion /////////////////////////////
	//**********************************************************************************

	fmistatus = fmi2_import_terminate(fmu);

	fmi2_import_free_instance(fmu);

	return true;
}





